#include <flight_controller_tcp.hpp>
#include <msg_flight_data_s.hpp>
#include <msg_param_s.hpp>
#include <rtime.hpp>

TcpServerClient* FlightControllerTcp::FlightControllerTcpCreator()
{
	return new FlightControllerTcp();
}

FlightControllerTcp::FlightControllerTcp()
	: m_DataSeqRecv(0)
	, m_DataTypeRecv(0)
	, m_SizeRecv(0)
{
	m_BuffRecv = (char*)malloc(BUFF_FIFO_SIZE);
}

FlightControllerTcp::~FlightControllerTcp()
{
	free(m_BuffRecv);
}

void FlightControllerTcp::RecvData(char* data, int size)
{
	if (size <= 0)
	{
		return;
	}

#if 0
	printf("recv %d:\n", size);
	for (int i = 0; i < size; i++)
	{
		printf("%02x ", (uint8_t)data[i]);
	}
	printf("\n");
#endif

	m_BuffFifo.Append(data, size);
	while (1)
	{
		if (m_Protocol.Unpack(m_BuffRecv, &m_SizeRecv, &m_DataSeqRecv, &m_DataTypeRecv, &m_BuffFifo) != 0)
		{
			break;
		}

		switch (m_DataTypeRecv)
		{
			case P_TYPE_HEART:
				break;

			case P_TYPE_FLIGHT_DATA:
				if (m_SizeRecv == sizeof(msg_flight_data_s))
				{
					msg_flight_data_s* p = (msg_flight_data_s*)m_BuffRecv;
					printf("%+8.4lf %+8.4lf %+8.4lf ", p->attitude.angles[0], p->attitude.angles[1], p->attitude.angles[2]);
					printf("%+8.4lf %+8.4lf\n", p->velocity_setpoint.vel[2], p->attitude.z_vel);
					__UNUSED(p)
				}
				break;

			case P_TYPE_PARAM_GET:
				if (m_SizeRecv == sizeof(msg_param_s))
				{
					msg_param_s* p = (msg_param_s*)m_BuffRecv;
					LOGI("get param %s %s\n", p->name, p->value);
				}

			default:
				break;
		}
	}
}